/*******************************************************************************
*   hone_markers : Publish a pointing markers for RViz
*   License      : BSD License
*   Copyright(c) 2011 Yoonseok Pyo.  All rights reserved.
*******************************************************************************/

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <sstream>
#include <cmath>

int main( int argc, char** argv )
{
    ros::init(argc, argv, "pointing_markers");
    ros::NodeHandle n;
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("pointing_markers", 1);

    ros::Rate r(60);

    tf::TransformListener listener;

    tf::StampedTransform coordinate_elbow_l;
    tf::StampedTransform coordinate_hand_l;
    tf::StampedTransform coordinate_elbow_r;
    tf::StampedTransform coordinate_hand_r;

    geometry_msgs::Point p[8]; 
    geometry_msgs::Point position_hand_l;
    geometry_msgs::Point position_goal_l;
    geometry_msgs::Point position_hand_r;
    geometry_msgs::Point position_goal_r;
    geometry_msgs::Point point_goal_l;
    geometry_msgs::Point point_goal_r;
   
    while (ros::ok())
    {
        visualization_msgs::Marker box_marker, pointing_marker[2], goal_marker[2], goal_text_marker[2];

        box_marker.header.frame_id      = pointing_marker[0].header.frame_id     = pointing_marker[1].header.frame_id     = goal_marker[0].header.frame_id      = goal_marker[1].header.frame_id = "/hone/openni_depth_frame";
        box_marker.header.stamp         = pointing_marker[0].header.stamp        = pointing_marker[1].header.stamp        = goal_marker[0].header.stamp         = goal_marker[1].header.stamp = ros::Time::now();
        box_marker.action               = pointing_marker[0].action              = pointing_marker[1].action              = goal_marker[0].action               = goal_marker[1].action = visualization_msgs::Marker::ADD;
        box_marker.pose.orientation.w   = pointing_marker[0].pose.orientation.w  = pointing_marker[1].pose.orientation.w  = goal_marker[0].pose.orientation.w   = goal_marker[1].pose.orientation.w = 1.0;

        goal_text_marker[0].header.frame_id      = goal_text_marker[1].header.frame_id = "/hone/openni_depth_frame";
        goal_text_marker[0].header.stamp         = goal_text_marker[1].header.stamp = ros::Time::now();
        goal_text_marker[0].action               = goal_text_marker[1].action = visualization_msgs::Marker::ADD;
        goal_text_marker[0].pose.orientation.w   = goal_text_marker[1].pose.orientation.w = 1.0;

        box_marker.ns                   = "box" ;
        pointing_marker[0].ns           = pointing_marker[1].ns = "pointing";
        goal_marker[0].ns               = goal_marker[1].ns     = "goal";
        goal_text_marker[0].ns          = goal_text_marker[1].ns= "goal_text";
        
        box_marker.id           = 0;
        pointing_marker[0].id   = 0;
        pointing_marker[1].id   = 1;
        goal_marker[0].id       = 0;
        goal_marker[1].id       = 1;
        goal_text_marker[0].id  = 0;
        goal_text_marker[1].id  = 1;

        box_marker.type                                     = visualization_msgs::Marker::LINE_LIST;
        pointing_marker[0].type = pointing_marker[1].type   = visualization_msgs::Marker::ARROW;
        goal_marker[0].type     = goal_marker[1].type       = visualization_msgs::Marker::POINTS;
        goal_text_marker[0].type= goal_text_marker[1].type  = visualization_msgs::Marker::TEXT_VIEW_FACING;

        box_marker.scale.x          = 0.1;
        pointing_marker[0].scale.x  = pointing_marker[1].scale.x    = 0.04;
        pointing_marker[0].scale.y  = pointing_marker[1].scale.y    = 0.08;
        goal_marker[0].scale.x      = goal_marker[1].scale.x        = 0.2;
        goal_marker[0].scale.y      = goal_marker[1].scale.y        = 0.2;
        goal_text_marker[0].scale.z = goal_text_marker[1].scale.z        = 0.4;

        box_marker.color.g = 1.0;
        box_marker.color.a = 1.0;

        pointing_marker[0].color.r  = 1.0;
        pointing_marker[0].color.g  = 1.0;
        pointing_marker[0].color.a  = 1.0;

        pointing_marker[1].color.r  = 1.0;
        pointing_marker[1].color.g  = 1.0;
        pointing_marker[1].color.a  = 1.0;

        goal_marker[0].color.r = 1.0;
        goal_marker[0].color.b = 1.0;
        goal_marker[0].color.a = 1.0;

        goal_marker[1].color.r = 1.0;
        goal_marker[1].color.g = 0.5;
        goal_marker[1].color.a = 1.0;

        goal_text_marker[0].color.r = 1.0;
        goal_text_marker[0].color.a = 1.0;

        goal_text_marker[1].color.r = 1.0;
        goal_text_marker[1].color.a = 1.0;

        pointing_marker[0].lifetime = ros::Duration(0);
        pointing_marker[1].lifetime = ros::Duration(0);

        //  coordinate box
        p[0].x =  5.0;  p[0].y =  5.0;   p[0].z =  5.0;
        p[1].x =  5.0;  p[1].y =  5.0;   p[1].z = -5.0;
        p[2].x = -5.0;  p[2].y =  5.0;   p[2].z = -5.0;
        p[3].x = -5.0;  p[3].y =  5.0;   p[3].z =  5.0;
        p[4].x =  5.0;  p[4].y = -2.0;   p[4].z =  5.0;
        p[5].x =  5.0;  p[5].y = -2.0;   p[5].z = -5.0;
        p[6].x = -5.0;  p[6].y = -2.0;   p[6].z = -5.0;
        p[7].x = -5.0;  p[7].y = -2.0;   p[7].z =  5.0;

        box_marker.points.push_back(p[0]);
        box_marker.points.push_back(p[1]);
        box_marker.points.push_back(p[1]);
        box_marker.points.push_back(p[2]);        
        box_marker.points.push_back(p[2]);
        box_marker.points.push_back(p[3]);
        box_marker.points.push_back(p[3]);
        box_marker.points.push_back(p[0]);
      
        box_marker.points.push_back(p[0]);
        box_marker.points.push_back(p[4]);
        box_marker.points.push_back(p[1]);
        box_marker.points.push_back(p[5]);
        box_marker.points.push_back(p[2]);
        box_marker.points.push_back(p[6]);
        box_marker.points.push_back(p[3]);
        box_marker.points.push_back(p[7]);

        box_marker.points.push_back(p[4]);
        box_marker.points.push_back(p[5]);
        box_marker.points.push_back(p[5]);
        box_marker.points.push_back(p[6]);
        box_marker.points.push_back(p[6]);
        box_marker.points.push_back(p[7]);
        box_marker.points.push_back(p[7]);
        box_marker.points.push_back(p[4]);

        marker_pub.publish(box_marker);
       

        //  coordinate hand
        float VectorT = 10.0;

        bool  check_transform_of_elbow_l = listener.waitForTransform("/hone/openni_depth_frame" , "/hone/left_elbow", ros::Time(0), ros::Duration(1));
        bool  check_transform_of_hand_l  = listener.waitForTransform("/hone/openni_depth_frame" , "/hone/left_hand",  ros::Time(0), ros::Duration(1));

        if (check_transform_of_elbow_l && check_transform_of_hand_l)
        {
            // markers of goal_marker (left)
            listener.lookupTransform("/hone/openni_depth_frame", "/hone/left_elbow", ros::Time(0), coordinate_elbow_l);
            listener.lookupTransform("/hone/openni_depth_frame", "/hone/left_hand" , ros::Time(0), coordinate_hand_l);
            
            btVector3 direction_l = coordinate_hand_l.getOrigin() - coordinate_elbow_l.getOrigin();

            std::vector <float> vector_of_hand_l (6, 0.0);

            vector_of_hand_l[0] = coordinate_hand_l.getOrigin().x();        // x1
            vector_of_hand_l[1] = coordinate_hand_l.getOrigin().y();        // y1
            vector_of_hand_l[2] = coordinate_hand_l.getOrigin().z();        // z1
            vector_of_hand_l[3] = direction_l.normalize().x();              // l
            vector_of_hand_l[4] = direction_l.normalize().y();              // m
            vector_of_hand_l[5] = direction_l.normalize().z();              // n

            position_hand_l.x  = vector_of_hand_l[0];
            position_hand_l.y  = vector_of_hand_l[1];
            position_hand_l.z  = vector_of_hand_l[2];
 
            position_goal_l.x = vector_of_hand_l[0] + VectorT * vector_of_hand_l[3];    // x = x1 + VectorT * l
            position_goal_l.y = vector_of_hand_l[1] + VectorT * vector_of_hand_l[4];    // y = y1 + VectorT * m
            position_goal_l.z = vector_of_hand_l[2] + VectorT * vector_of_hand_l[5];    // z = z1 + VectorT * n

            float VectorK_L = 0.0;

            VectorK_L = ( p[1].z - vector_of_hand_l[2] ) / vector_of_hand_l[5];
            
            point_goal_l.x = vector_of_hand_l[0] + VectorK_L * vector_of_hand_l[3];    // x = x1 + VectorK_L * l
            point_goal_l.y = vector_of_hand_l[1] + VectorK_L * vector_of_hand_l[4];    // y = y1 + VectorK_L * m
            point_goal_l.z = p[1].z;

            goal_marker[0].points.push_back(point_goal_l);
            marker_pub.publish(goal_marker[0]);

            // markers of goal_text_marker (left)
            std::stringstream coordinate_goal_l;
            coordinate_goal_l << "Coordinate" << "\nX: " << point_goal_l.x << "\nY: " << point_goal_l.z << "\nZ: "<< point_goal_l.y ;
            
            goal_text_marker[0].text = coordinate_goal_l.str();
            goal_text_marker[0].pose.position.x = point_goal_l.x;
            goal_text_marker[0].pose.position.y = point_goal_l.y;
            goal_text_marker[0].pose.position.z = point_goal_l.z;
            marker_pub.publish(goal_text_marker[0]);

            // markers of pointing_marker (left)
            if(position_hand_l.y > -0.2)
            {
                pointing_marker[0].action = visualization_msgs::Marker::ADD;

                pointing_marker[0].points.push_back(position_hand_l);
                pointing_marker[0].points.push_back(position_goal_l);
                marker_pub.publish(pointing_marker[0]);
            }
            else
            {
                pointing_marker[0].action = visualization_msgs::Marker::DELETE;

                pointing_marker[0].points.push_back(position_hand_l);
                pointing_marker[0].points.push_back(position_goal_l);
                marker_pub.publish(pointing_marker[0]);
            }                
        }

        bool check_transform_of_elbow_r = listener.waitForTransform("/hone/openni_depth_frame" , "/hone/right_elbow", ros::Time(0), ros::Duration(1));
        bool check_transform_of_hand_r  = listener.waitForTransform("/hone/openni_depth_frame" , "/hone/right_hand",  ros::Time(0), ros::Duration(1));

        if (check_transform_of_elbow_r && check_transform_of_hand_r)
        {
            // markers of goal_marker (right)
            listener.lookupTransform("/hone/openni_depth_frame", "/hone/right_elbow", ros::Time(0), coordinate_elbow_r);
            listener.lookupTransform("/hone/openni_depth_frame", "/hone/right_hand" , ros::Time(0), coordinate_hand_r);

            btVector3 direction_r = coordinate_hand_r.getOrigin() - coordinate_elbow_r.getOrigin();

            std::vector <float> vector_of_hand_r (6, 0.0);

            vector_of_hand_r[0] = coordinate_hand_r.getOrigin().x();
            vector_of_hand_r[1] = coordinate_hand_r.getOrigin().y();
            vector_of_hand_r[2] = coordinate_hand_r.getOrigin().z();
            vector_of_hand_r[3] = direction_r.normalize().x();
            vector_of_hand_r[4] = direction_r.normalize().y();
            vector_of_hand_r[5] = direction_r.normalize().z();

            position_hand_r.x  = vector_of_hand_r[0];
            position_hand_r.y  = vector_of_hand_r[1];
            position_hand_r.z  = vector_of_hand_r[2];

            position_goal_r.x = vector_of_hand_r[0] + VectorT * vector_of_hand_r[3];
            position_goal_r.y = vector_of_hand_r[1] + VectorT * vector_of_hand_r[4];
            position_goal_r.z = vector_of_hand_r[2] + VectorT * vector_of_hand_r[5];

            float VectorK_R = 0.0;

            VectorK_R = ( p[1].z - vector_of_hand_r[2] ) / vector_of_hand_r[5];
            
            point_goal_r.x = vector_of_hand_r[0] + VectorK_R * vector_of_hand_r[3];    // x = x1 + VectorK_R * l
            point_goal_r.y = vector_of_hand_r[1] + VectorK_R * vector_of_hand_r[4];    // y = y1 + VectorK_R * m
            point_goal_r.z = p[1].z;

            goal_marker[1].points.push_back(point_goal_r);
            marker_pub.publish(goal_marker[1]);

            // markers of goal_text_marker (right)
            std::stringstream coordinate_goal_r;
            coordinate_goal_r << "Coordinate" << "\nX: " << point_goal_r.x << "\nY: " << point_goal_r.z << "\nZ: "<< point_goal_r.y ;
            
            goal_text_marker[1].text = coordinate_goal_r.str();
            goal_text_marker[1].pose.position.x = point_goal_r.x;
            goal_text_marker[1].pose.position.y = point_goal_r.y;
            goal_text_marker[1].pose.position.z = point_goal_r.z;
            marker_pub.publish(goal_text_marker[1]);

            // markers of pointing_marker (right)
            if(position_hand_r.y > -0.2)
            {
                pointing_marker[1].action = visualization_msgs::Marker::ADD;
                pointing_marker[1].points.push_back(position_hand_r);
                pointing_marker[1].points.push_back(position_goal_r);
                marker_pub.publish(pointing_marker[1]);
            }else
            {
                pointing_marker[1].action = visualization_msgs::Marker::DELETE;
                pointing_marker[1].points.push_back(position_hand_r);
                pointing_marker[1].points.push_back(position_goal_r);
                marker_pub.publish(pointing_marker[1]);
            }
        }

        //ROS_INFO("%f, %f, %f, %f",position_hand_l.x, position_hand_l.y, position_hand_r.x, position_hand_r.y);

        r.sleep();
    }
}




